#include <btl/extra/Gui/OsgSceneMarkers.hpp>

#include <osgViewer/Viewer>
#include <osgGA/TrackballManipulator>
#include <osgText/Text>

#include <osg/Geode>
#include <osg/Geometry>
#include <osg/MatrixTransform>
#include <osg/Camera>
#include <osg/CameraView>

#include <Eigen/Dense>
#include <iostream>
#include <cassert>

using namespace btl::extra::gui;

void setupViewWindow(osg::View* view)
{
   osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
   traits->x = 40;
   traits->y = 40;
   traits->width = 640;
   traits->height = 480;
   traits->windowDecoration = true;
   traits->doubleBuffer = true;
   traits->sharedContext = 0;

   osg::ref_ptr<osg::GraphicsContext> gc = osg::GraphicsContext::createGraphicsContext(traits.get());

   osg::ref_ptr<osg::Camera> camera = new osg::Camera;
   camera->setGraphicsContext(gc.get());
   camera->setViewport(new osg::Viewport(0,0, traits->width, traits->height));
   GLenum buffer = traits->doubleBuffer ? GL_BACK : GL_FRONT;
   camera->setDrawBuffer(buffer);
   camera->setReadBuffer(buffer);
   camera->getViewMatrix().makeIdentity();
   camera->setViewMatrixAsLookAt(osg::Vec3d(20.0, 0.0, 0.0),osg::Vec3d(0.0,0.0,0.0), osg::Vec3d(0.0,-1.0,0.0)); 
//   camera->setViewMatrix(osg::Matrix::lookAt(osg::Vec3(0, 0, 20), osg::Vec3(0, 0, 0), osg::Vec3(0, -1, 0))); 
   view->addSlave(camera.get());
}

osg::Node* createCameraMarkers()
{
   osg::Group* poses = new osg::Group;

   const double size = 0.5;
   const double fov = 80.0 * (M_PI/180.0);
   const double radius = 20.0;
   const double aspect = 752.0 / 480.0;

   const int NUM_CAMERAS = 80;
   for(int i = 0; i < NUM_CAMERAS; ++i)
   {
      double angle = double(i) / double(NUM_CAMERAS) * (M_PI*2.0);

      Eigen::Quaterniond poseAngle
      = Eigen::Quaterniond(Eigen::AngleAxis<double>(angle, -1* Eigen::Vector3d::UnitY()));

      Eigen::Vector3d pos = poseAngle * Eigen::Vector3d(0.0, 0.0, -radius);
      Eigen::Affine3d pose = poseAngle * Eigen::Translation3d(pos); // what is pose used for?

      OsgCameraMarker* camMarker = new OsgCameraMarker(fov, aspect, size);

      osg::Geode* camNode = new osg::Geode;
      camNode->addDrawable(camMarker);

      osg::CameraView* camPose = new osg::CameraView;
      camPose->setFieldOfViewMode(osg::CameraView::HORIZONTAL);
      camPose->setFieldOfView(fov);
      camPose->setPosition(osg::Vec3d(pos(0), pos(1), pos(2)));
      camPose->setAttitude(osg::Quat(poseAngle.x(), poseAngle.y(), poseAngle.z(), poseAngle.w()));
      camPose->addChild(camNode);

      poses->addChild(camPose);
   }

   return poses;
}

int main(int argc, char** argv)
{
   // osg's default coordinate system isn't the one I want
   osg::MatrixTransform* root = new osg::MatrixTransform(
      osg::Matrixd(
         1.0, 0.0, 0.0, 0.0,
         0.0,-1.0, 0.0, 0.0,
         0.0, 0.0,-1.0, 0.0,
         0.0, 0.0, 0.0, 1.0));

   // disable lighting
   osg::StateSet* stateSet = new osg::StateSet;
   stateSet->setMode(GL_LIGHTING, GL_FALSE);
   stateSet->setMode(GL_BLEND, GL_TRUE);
   stateSet->setMode(GL_LINE_SMOOTH, GL_TRUE);
   //stateSet->setAttribute(new osg::Hint());
   root->setStateSet(stateSet);

   // add a coordinate frame marker
   osg::Geode* coordFrameMarker = new osg::Geode;
   coordFrameMarker->addDrawable(new OsgCoordinateFrameMarker(10.0));
   root->addChild(coordFrameMarker);

   // add a bunch of camera pose markers
   root->addChild(createCameraMarkers());

   osgViewer::Viewer viewer;
   setupViewWindow(&viewer);
   viewer.setSceneData(root);
//   viewer.setCameraManipulator(new osgGA::TrackballManipulator());
//   viewer.getCameraManipulator()->setHomePosition( osg::Vec3d(20.0, 0.0, 0.0),osg::Vec3d(0.0,0.0,0.0), osg::Vec3d(0.0,-1.0,0.0) );
   return viewer.run();
}
